// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include <cmath>
#include <iostream>

using namespace std;

// 三条臂的长度
float d = 1247, l1 = 1461.35, l2 = 2340;
// 运动学中的关节角
float joint1, joint2, joint3;
// 长度
float len;
float len2;
// 实际斜臂高度
float high2 = 1190.1;
// 正向运动学高度
float high;
// 大臂角度
float alpha = 15 / 180 * M_PI;
// 赋值的，供逆解使用的坐标值
float x_data, y_data, z_data;
float temp1, temp2, temp3;

// 正向运动学实现
void FK_SOLV(const float theta1_f, const float theta2_f, const float theta3_f, float &fx, float &fy, float &fz)
{
    joint1 = theta1_f / 180.0 * M_PI;
    joint2 = (180.0 - 15 - theta2_f) / 180.0 * M_PI;
    joint3 = (180.0 - theta3_f) / 180.0 * M_PI;
    // len = l1 * sin(joint2) + l2 * sin(joint2 + joint3) - d * sin(alpha);
    len = l1 * sin(joint2) + l2 * sin(joint2 + joint3) - 322.74;
    // high = l1 * cos(joint2) + l2 * cos(joint2 + joint3) + d * cos(alpha);
    high = l1 * cos(joint2) + l2 * cos(joint2 + joint3) + 1204.51;

    // std::cout << "theta2_f == " << theta2_f << "\n";
    // std::cout << "joint2 == " << joint2 << "\n";
    // std::cout << "joint3 == " << joint3 << "\n";
    // std::cout << "len == " << len << "\n";
    // std::cout << "l1 * cos(joint2) ==" << l1 * cos(joint2) << "\n";
    // std::cout << "l2 * cos(joint2 + joint3) == " << l2 * cos(joint2 + joint3) << "\n";
    fx = len * cos(joint1);
    fy = len * sin(joint1);
    fz = high;
}

// 逆向运动学实现
void IK_SOLV(const float x, const float y, const float z, float &theta1_I, float &theta2_I, float &theta3_I)
{
    // 底座角
    theta1_I = atan(y / x);
    // 计算长度
    // len2 = sqrt(x * x + y * y) + d * sin(11 / 180 * M_PI);
    len2 = sqrt(x * x + y * y) + 322.74;
    // 二号关节角度
    if (high2 > z)
    {
        float short_ = high2 - z;
        float slant_ = sqrt(short_ * short_ + len2 * len2);
        float theta2_1 = atan(len2 / short_);
        float theta2_2 = acos(fminl(fmaxl((l1 * l1 + slant_ * slant_ - l2 * l2) / (2.0 * l1 * slant_), -1.0), 1.0));
        theta2_I = theta2_1 + theta2_2 - (15.0 / 180.0 * M_PI);
        std::cout << "high > z\n";
    }
    else if (high2 < z)
    {
        float short_ = z - high2;
        float slant_ = sqrt(short_ * short_ + len2 * len2);
        float theta2_1 = atan(short_ / len2);
        float theta2_2 = acos(fminl(fmaxl((l1 * l1 + slant_ * slant_ - l2 * l2) / (2.0 * l1 * slant_), -1.0), 1.0));
        theta2_I = theta2_1 + theta2_2 + (90.0 - 15.0) / 180.0 * M_PI;
        std::cout << "high < z\n";
    }
    else
    {
        float short_ = 0;
        float slant_ = sqrt(short_ * short_ + len2 * len2);
        float theta2_2 = acos(fminl(fmaxl((l1 * l1 + slant_ * slant_ - l2 * l2) / (2.0 * l1 * slant_), -1.0), 1.0));
        theta2_I = theta2_2 + (90.0 - 15.0) / 180.0 * M_PI;
        std::cout << "high = z\n";
    }
    // 三号关节角度
    float short_2 = std::abs(z - high2);
    float slant_2 = sqrt(short_2 * short_2 + len2 * len2);
    theta3_I = acos(fminl(fmaxl((l1 * l1 + l2 * l2 - slant_2 * slant_2) / (2.0 * l1 * l2), -1.0), 1.0));
    // 转换成角度制
    theta1_I = theta1_I * (180.0 / M_PI);
    theta2_I = theta2_I * (180.0 / M_PI);
    theta3_I = theta3_I * (180.0 / M_PI);
}

// 已知三角形的三边 a、b 和 c，要求夹角 C
void to_cos(const float c_a, const float c_b, const float c_c, float &theta_c)
{
    float numerator = c_a * c_a + c_b * c_b - c_c * c_c;
    float denominator = 2.0f * c_a * c_b;
    float cos_c = numerator / denominator;
    theta_c = acosf(cos_c) * 180.0f / static_cast<float>(M_PI); // 弧度转角度
}
// 已知三角形的两边 a 和 b 以及它们之间的夹角 c（以度为单位），要求第三边 c。
void to_length(const float l_a, const float l_b, const float l_theta_c, float &l_c)
{
    float c_rad = l_theta_c * M_PI / 180.0;
    float c = sqrt(l_a * l_a + l_b * l_b - 2 * l_a * l_b * cos(c_rad));
    l_c = c;
}
//
//
//
// ----------------------------------------------------------ROS2部分----------------------------------------------------------
class Arm_ik : public rclcpp::Node
{
public:
    Arm_ik() : Node("arm_ik_node_cpp")
    {
        // 初始化成员变量
        float x_data = 0.0;
        float y_data = 0.0;
        float z_data = 0.0;
        float j1_data = 0.0;
        float j2_data = 0.0;
        float j3_data = 0.0;

        // 初始化标志变量
        x_updated = false;
        y_updated = false;
        z_updated = false;

        // 实例化回调组
        callback_group_x = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_y = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_z = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_j1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_j2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_j3 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        updata_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        // 创建订阅选项，并设置回调组
        rclcpp::SubscriptionOptions options_x;
        options_x.callback_group = callback_group_x;
        rclcpp::SubscriptionOptions options_y;
        options_y.callback_group = callback_group_y;
        rclcpp::SubscriptionOptions options_z;
        options_z.callback_group = callback_group_z;
        rclcpp::SubscriptionOptions options_j1;
        options_j1.callback_group = callback_group_j1;
        rclcpp::SubscriptionOptions options_j2;
        options_j2.callback_group = callback_group_j2;
        rclcpp::SubscriptionOptions options_j3;
        options_j3.callback_group = callback_group_j3;

        // 创建订阅方
        // --订阅来自控制方的x,y,z坐标
        this->x_sub_ = this->create_subscription<std_msgs::msg::Float64>("x_goat", 5, std::bind(&Arm_ik::x_cb, this, std::placeholders::_1), options_x);
        this->y_sub_ = this->create_subscription<std_msgs::msg::Float64>("y_goat", 5, std::bind(&Arm_ik::y_cb, this, std::placeholders::_1), options_y);
        this->z_sub_ = this->create_subscription<std_msgs::msg::Float64>("z_goat", 5, std::bind(&Arm_ik::z_cb, this, std::placeholders::_1), options_z);
        // --订阅来自编码器和 ADC的数据，正向解析
        this->joint1_sub = this->create_subscription<std_msgs::msg::Int32>("joint1_p", 5, std::bind(&Arm_ik::j1_cb, this, std::placeholders::_1), options_j1);
        this->joint2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_p", 5, std::bind(&Arm_ik::j2_cb, this, std::placeholders::_1), options_j2);
        this->joint3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_p", 5, std::bind(&Arm_ik::j3_cb, this, std::placeholders::_1), options_j3);

        // 创建发布方
        // --发布逆解求的的关节数据角度、行程(发布话题暂定),-(来自QT或者arm_control-joint程序控制需要),
        // 这里发布的数据是直接操作关节动作的
        j1_pub = this->create_publisher<std_msgs::msg::Float64>("joint1_gp", 5);
        j2_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_gp", 5);
        j3_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_gp", 5);

        // 创建定时器，用于逐步更新关节数据
        timer = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&Arm_ik::update_joint_data, this), updata_group);
    }
    // 逆解计算
    void calculate_IK()
    {

        // 确保 x_data, y_data, z_data 已经被赋值
        if (x_updated && y_updated && z_updated)
        {
            IK_SOLV(x_data, y_data, z_data, theta1_I, theta2_I, theta3_I);
            if (theta1_I < 0)
            {
                theta1_I = theta1_I + 180;
            }
            std::cout << "逆向运动学结果:\n"
                      << "theta1 = " << theta1_I << "\n"
                      << "theta2 = " << theta2_I << "\n"
                      << "theta3 = " << theta3_I << "\n";
            // 对逆解获得的角度值，转换成油缸的行程
            if (l2 > 670.51 + 1008.6)
            {
                l2 = 670.51 + 1008.6;
            }
            to_length(670.51, 1008.6, theta2_I, l2);
            g2 = l2 - 1252;

            if (l3 > 300 + 800)
            {
                l3 = 300 + 800;
            }
            to_length(300, 800, theta3_I, l3);
            g3 = l3 - 718.5;
            std::cout << "油缸行程为:\n"
                      << "g2 = " << g2 << "\n"
                      << "g3 = " << g3 << "\n"
                      << "\n";
            // joint2_p.data = g2;
            // joint3_p.data = g3;
            temp1 = theta1_I;
            temp2 = g2;
            temp3 = g3;

            // // 初始化消息对象
            joint1_p.data = temp1;
            joint2_p.data = temp2;
            joint3_p.data = temp3;
            j1_pub->publish(joint1_p);
            j2_pub->publish(joint2_p);
            j3_pub->publish(joint3_p);

            // 重置标志变量
            x_updated = false;
            y_updated = false;
            z_updated = false;
        }
    }
    // 正解计算
    void calculate_FK()
    {
        // ROS2发布正解数据定义
        // auto joint1_j = std_msgs::msg::Float64();
        // auto joint2_j = std_msgs::msg::Float64();
        // auto joint3_j = std_msgs::msg::Float64();
        // 确保已经被赋值
        if (j1_updated && j2_updated && j3_updated)
        {
            // 处理获取到的数据，转换成对应关节角度
            l2_f = j2_data + 1252;
            std::cout << "j2_data = " << j2_data << "\n";
            std::cout << "l2_f = " << l2_f << "\n";
            if (l2_f > 1043.75 + 670.51)
            {
                l2_f = 1043.75 + 670.51;
            }
            to_cos(1043.75, 670.51, l2_f, theta2_F);

            l3_f = j3_data + 718.5;
            // std::cout << l3_f << "\n";
            if (l3_f > 800 + 300)
            {
                l3_f = 800 + 300;
            }
            to_cos(800, 300, l3_f, theta3_F);
            // to_cos(78, 28, 106, theta3_F);

            FK_SOLV(j1_data, theta2_F, theta3_F, fx, fy, fz);
            // FK_SOLV(0, 155.48, 100.447, fx, fy, fz);

            std::cout << "正向运动学结果:\n"
                      << "x = " << fx << "\n"
                      << "y = " << fy << "\n"
                      << "z = " << fz << "\n";
            std::cout << "各个角度为:\n"
                      << "theta1 = " << j1_data << "\n"
                      << "theta2 = " << theta2_F << "\n"
                      << "theta3 = " << theta3_F << "\n";
            // 重置标志变量
            j1_updated = false;
            j2_updated = false;
            j3_updated = false;
        }
    }

private:
    // 成员变量存储要发布的消息
    std_msgs::msg::Float64 joint1_p;
    std_msgs::msg::Float64 joint2_p;
    std_msgs::msg::Float64 joint3_p;

    // 逆解所需的x、y、z坐标，来自qt或者control程序
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr x_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr y_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr z_sub_;

    // 一号关节旋转量，g2,g3油缸行程发布（逆解所求）
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j1_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j2_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr j3_pub;

    // 一号编码器，二三号ADC的数据
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3_sub;

    // 声明多线程回调组
    rclcpp::CallbackGroup::SharedPtr callback_group_x;
    rclcpp::CallbackGroup::SharedPtr callback_group_y;
    rclcpp::CallbackGroup::SharedPtr callback_group_z;
    rclcpp::CallbackGroup::SharedPtr callback_group_j1;
    rclcpp::CallbackGroup::SharedPtr callback_group_j2;
    rclcpp::CallbackGroup::SharedPtr callback_group_j3;
    rclcpp::CallbackGroup::SharedPtr updata_group;

    //
    rclcpp::TimerBase::SharedPtr timer;

    // 逆解求得的角度
    float theta1_I, theta2_I, theta3_I;
    // 正解余弦转换的角度
    float theta1_F, theta2_F, theta3_F;
    // 正解求得的坐标值
    float fx, fy, fz;
    // 逆解直接转换的长度
    float l2, l3;
    // 需要发送的液压缸行程
    float g2, g3;
    // 正解所需的余弦定理求解到的长度
    float l2_f, l3_f;

    float x_data = 0.0;
    float y_data = 0.0;
    float z_data = 0.0;
    float j1_data = 0.0;
    float j2_data = 0.0;
    float j3_data = 0.0;

    bool x_updated = false;
    bool y_updated = false;
    bool z_updated = false;
    bool j1_updated = false;
    bool j2_updated = false;
    bool j3_updated = false;

    // 订阅并赋值x、y、z(逆解)；
    void x_cb(const std_msgs::msg::Float64::SharedPtr x_msg)
    {
        x_data = x_msg->data;
        x_updated = true; // 更新标志变量
        calculate_IK();   // 在回调函数中调用逆解计算
    }
    void y_cb(const std_msgs::msg::Float64::SharedPtr y_msg)
    {
        y_data = y_msg->data;
        y_updated = true; // 更新标志变量
        calculate_IK();   // 在回调函数中调用逆解计算
    }
    void z_cb(const std_msgs::msg::Float64::SharedPtr z_msg)
    {
        z_data = z_msg->data;
        z_updated = true; // 更新标志变量
        calculate_IK();   // 在回调函数中调用逆解计算
    }

    // 订阅并赋值j1、j2、j3(正解)；
    void j1_cb(const std_msgs::msg::Int32::SharedPtr j1_msg)
    {
        j1_data = float(j1_msg->data);
        j1_updated = true;
        calculate_FK();
    }
    void j2_cb(const std_msgs::msg::Float64::SharedPtr j2_msg)
    {
        j2_data = j2_msg->data;
        j2_updated = true;
        calculate_FK();
    }
    void j3_cb(const std_msgs::msg::Float64::SharedPtr j3_msg)
    {
        j3_data = j3_msg->data;
        j3_updated = true;
        calculate_FK();
    }

    void update_joint_data()
    {
        // joint1_p.data = temp1;
        // j1_pub->publish(joint1_p);
        // // std::cout<<"temp1"<<temp1<<"\n";
        // joint2_p.data = temp2;
        // j2_pub->publish(joint2_p);
        // std::cout << "temp2" << temp2 << "\n";
        // joint2_p.data = temp2;
        // j2_pub->publish(joint2_p);
        // // std::cout << "temp3" << temp3 << "\n";
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 6, true);
    // 4.调用spain函数，并传入节点对象指针；
    auto node = std::make_shared<Arm_ik>();
    executor.add_node(node);
    executor.spin();
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}
